#include "Serial_RX.h"

u8 Fd_data[64];
u8 Fd_rsimu[64];
u8 Fd_rsahrs[56];
int rs_imutype =0;
int rs_ahrstype =0;
IMUData_Packet_t IMUData_Packet;
AHRSData_Packet_t AHRSData_Packet;

/*角度传感器变量定义*/
struct SAcc 		stcAcc;
struct SGyro 		stcGyro;
struct SAngle 	stcAngle;

//每个串口需要设置一次，接收不定长的数据，只接收~与0D0A之间的数据
/**************************************************************************
函数功能：串口1接收函数，帧头-‘~’，帧尾-“\r\n”
入口参数：串口1接收的单字节数据
返回  值：无
**************************************************************************/
void UART1_Receive(uint8_t RXBUF)
{
//		u8 Receive_Flag[6] = {0xa0,0xb0,'S','1',0x0d,0x0a},i = 0;		//收到数据后返回
	uint16_t UART1_Max_Recv_Len = 20;                           //最大接收长度
	static uint16_t UART1_STA = 0;                              //数据长度计数，同时用最后一位表示接收0A,倒数第二位表示接收0D
	static uint8_t UART1_RX_BUF[20],Uart_FLag = 0;
	RXBUF == '~' ? Uart_FLag = 1 : 0;
	if((UART1_STA&0x8000)==0 && Uart_FLag)                      //未接收到0D0A[只有0D不能使用，0A和~在第一个~和0D之间可以使用]
	{
			if(UART1_STA&0x4000)                                    //上一位接收到0x0d
			{
					if(RXBUF!=0x0a)UART1_STA=0;                         //接收到0D后下一位不是0A,重新开始
					else UART1_STA|=0x8000;                             //接收到0X0A
			}
			else                                                    //还没收到0X0D
			{
					if(RXBUF==0x0d)UART1_STA|=0x4000;                   //接收到0X0D
					else
					{
							if(RXBUF!=0x0A && RXBUF!=' ')
							{
									UART1_RX_BUF[UART1_STA&0X3FFF]=RXBUF;         //存储数据
									UART1_STA++;
							}                                 //接收数据计数
							if(UART1_STA>(UART1_Max_Recv_Len-1))UART1_STA=0;//接收数据长度错误,重新开始接收
					}
			}
	}                                                           //接收到0D0A，处理通信数据
	if(UART1_STA&0x8000 && UART1_STA&0x4000 && Uart_FLag)
	{
		//处理函数//
		UARTData_Process(UART1_RX_BUF,UART1_STA&0X3FFF);
		UART1_STA = 0;                                          //处理完成，计数清零
		Uart_FLag = 0;
	}
}

/**************************************************************************
函数功能：串口2接收函数，帧头-‘~’，帧尾-“\r\n”
入口参数：串口2接收的单字节数据
返回  值：无
**************************************************************************/
void UART2_Receive(uint8_t RXBUF)
{
//	u8 Receive_Flag[6] = {0xa0,0xb0,'S','1',0x0d,0x0a},i = 0;		//收到数据后返回
	uint16_t UART2_Max_Recv_Len = 20;                           //最大接收长度
	static uint16_t UART2_STA = 0;                              //数据长度计数，同时用最后一位表示接收0A,倒数第二位表示接收0D
	static uint8_t UART2_RX_BUF[20],Uart_FLag = 0;
	RXBUF == '~' ? Uart_FLag = 1 : 0;
	if((UART2_STA&0x8000)==0 && Uart_FLag)                      //未接收到0D0A[只有0D不能使用，0A和~在第一个~和0D之间可以使用]
	{
			if(UART2_STA&0x4000)                                    //上一位接收到0x0d
			{
					if(RXBUF!=0x0a)UART2_STA=0;                         //接收到0D后下一位不是0A,重新开始
					else UART2_STA|=0x8000;                             //接收到0X0A
			}
			else                                                    //还没收到0X0D
			{
					if(RXBUF==0x0d)UART2_STA|=0x4000;                   //接收到0X0D
					else
					{
							if(RXBUF!=0x0A && RXBUF!=' ')
							{
									UART2_RX_BUF[UART2_STA&0X3FFF]=RXBUF;         //存储数据
									UART2_STA++;
							}                                 //接收数据计数
							if(UART2_STA>(UART2_Max_Recv_Len-1))UART2_STA=0;//接收数据长度错误,重新开始接收
					}
			}
	}                                                           //接收到0D0A，处理通信数据
	if(UART2_STA&0x8000 && UART2_STA&0x4000 && Uart_FLag)
	{
		//处理函数//
		UARTData_Process(UART2_RX_BUF,UART2_STA&0X3FFF);
		UART2_STA = 0;                                          //处理完成，计数清零
		Uart_FLag = 0;
	}
}

/**************************************************************************
函数功能：串口接收数据处理函数，通过特定协议修改3轴速度
入口参数：串口接收的一段数据与其长度
返回  值：无
**************************************************************************/
int UARTData_Process(uint8_t *UART_RX_BUF,uint16_t UART_STA)
{
		char i = 0;
		for(i=0;i<UART_STA;i++)
		{
			switch(UART_RX_BUF[i])
			{
					case 'W':
							V_X = CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1);
							return 0;
					case 'w':
							V_X = -1*CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1);
							return 0;
					case 'Q':
							V_Y = -1*CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1);
							return 0;
					case 'E':
							V_Y = CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1);
							return 0;
					case 'A':
							Target_Z = Angle[2] + CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1);
							//Target_Z += CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1);
							if(Target_Z >=180)
							{
								Target_Z -= 360;
							}
							else if(Target_Z <= -180)
							{
								Target_Z += 360 ;
							}
							return 0;
					case 'S':
							V_X = 0;
							V_Y = 0;	
							flag_S = 0;
							S_Y = Target_S_Y;
							S_X = Target_S_X;
							return 0;
					case 's':
							V_X = 0;
							V_Y = 0;	
							Target_Z = Angle[2];
							flag_S = 0;
							S_Y = Target_S_Y;
							S_X = Target_S_X;
							return 0;
					case 'D':
							Target_Z =Angle[2] - CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1);
							if(Target_Z >=180)
							{
								Target_Z -= 360;
							}
							else if(Target_Z <= -180)
							{
								Target_Z += 360 ;
							}
							return 0;
					case 'd':
							if(CharToNum(&UART_RX_BUF[i+1],UART_STA-i-1)-0.1<=0.0001)
							{
								Target_Z =Angle[2] - 180;
								if(Target_Z >=180)
								{
									Target_Z -= 360;
								}
								else if(Target_Z <= -180)
								{
									Target_Z += 360 ;
								}
							}
							return 0;
					case 'X':			//多种数据传输以X开头
							DataS_Process(UART_RX_BUF,UART_STA);
							flag_S = 1;
							return 0;
					case 'Y':			//多种数据传输以Y开头
							DataS_Process(UART_RX_BUF,UART_STA);
							flag_S = 1;
							return 0;
					case 'C':
							Car_Status = !Car_Status;
							return 0;
					default:break;
			}		
		}
    return 0;
}
/**************************************************************************
函数功能：
入口参数：
返回  值：
**************************************************************************/
int DataS_Process(uint8_t *UART_RX_BUF,uint16_t UART_STA)
{
		u8 i = 0,j = 0,k = 0;		//数组的位数、数据的个数 
		u8 Data_STA[5];		//记录第j个数据最后一位的位置
		for(i=0;i<UART_STA;i++)
		{
			switch(UART_RX_BUF[i])
			{
					case '~':	
						/*记录i，第j个数据'~'的位置，第0个数据=0*/
						Data_STA[j] = i;
						/*'~'的个数，共j个数*/
						j++;
						break;
					default:break;
			}		
		}
		Data_STA[j] = UART_STA;		
		/*给j个数据进行赋值*/
		for(i=0;i<j;i++)
		{
			for(k=Data_STA[i];k<Data_STA[i+1];k++)
			{
				switch(UART_RX_BUF[k])
				{
					case 'X':
							Target_S_X  = S_X + CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1);
							break;
					case 'Y':
							Target_S_Y  = S_Y + 1.23*CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1);
							break;
					case 'W':
							if(Target_S_X > S_X)
								V_X = float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1));
							else
								V_X = -1*float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1)); 
							break;
					case 'w':
							if(Target_S_X > S_X)
								V_X = float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1));
							else
								V_X = -1*float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1)); 
							break;
					case 'Q':
							if(Target_S_Y > S_Y)
								V_Y = float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1));
							else
								V_Y = -1*float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1));
							break;
					case 'E':
							if(Target_S_Y > S_Y)
								V_Y = float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1));
							else
								V_Y = -1*float_abs(CharToNum(&UART_RX_BUF[k+1],Data_STA[i+1]-k-1));
							break;
					default:break;
				}
			}
		}
    return 0;
}
/**************************************************************************
函数功能：jy901数据处理函数
入口参数：串口3接收的单个字节
返回  值：无
**************************************************************************/
void CopeSerial2Data(unsigned char ucData)
{
	static unsigned char ucRxBuffer[250];
	static unsigned char ucRxCnt = 0;	
	
//	LED_REVERSE();					//接收到数据，LED灯闪烁一下
	ucRxBuffer[ucRxCnt++]=ucData;	//将收到的数据存入缓冲区中
	if (ucRxBuffer[0]!=0x55) //数据头不对，则重新开始寻找0x55数据头
	{
		ucRxCnt=0;
		return;
	}
	if (ucRxCnt<11) {return;}//数据不满11个，则返回
	else
	{
		switch(ucRxBuffer[1])//判断数据是哪种数据，然后将其拷贝到对应的结构体中，有些数据包需要通过上位机打开对应的输出后，才能接收到这个数据包的数据
		{
			//memcpy为编译器自带的内存拷贝函数，需引用"string.h"，将接收缓冲区的字符拷贝到数据结构体里面，从而实现数据的解析。
			case 0x51:	memcpy(&stcAcc,&ucRxBuffer[2],8);break;
			case 0x52:	memcpy(&stcGyro,&ucRxBuffer[2],8);break;
			case 0x53:	memcpy(&stcAngle,&ucRxBuffer[2],8);break;
		}
		ucRxCnt=0;//清空缓存区
	}
}
/**************************************************************************
函数功能：N100数据处理函数
入口参数：串口3接收的单个字节
返回  值：无
**************************************************************************/
void N100_Receive(u8 Usart_Receive)
{
	static u8 Count=0;
	static u8 last_rsnum=0;
	static u8 rsimu_flag=0;
	static u8 rsacc_flag=0;
	
	Fd_data[Count]=Usart_Receive;  //串口数据填入数组
	if(((last_rsnum==FRAME_END)&&(Usart_Receive == FRAME_HEAD))||Count>0)
	{
		Count++; 
		if((Fd_data[1]==TYPE_IMU)&&(Fd_data[2]==IMU_LEN))
			rsimu_flag=1;
		if((Fd_data[1]==TYPE_AHRS)&&(Fd_data[2]==AHRS_LEN))
			rsacc_flag=1;
	}
	else 
		Count=0;
	last_rsnum=Usart_Receive;
	
	if(rsimu_flag==1 && Count==IMU_RS) //将本帧数据保存至Fd_rsimu数组中
	{
		Count=0;
		rsimu_flag=0;
		rs_imutype=1;
		if(Fd_data[IMU_RS-1]==FRAME_END) //帧尾校验
		memcpy(Fd_rsimu, Fd_data, sizeof(Fd_data));       
	}
	if(rsacc_flag==1 && Count==AHRS_RS) //
	{
		Count=0;
		rsacc_flag=0;
		rs_ahrstype=1;
		if(Fd_data[AHRS_RS-1]==FRAME_END)
		memcpy(Fd_rsahrs, Fd_data, sizeof(Fd_data));
	}
	if(rs_ahrstype==1)
	{		
		if(Fd_rsahrs[1]==TYPE_AHRS&&Fd_rsahrs[2]==AHRS_LEN)
		{	
			AHRSData_Packet.Roll=DATA_Trans(Fd_rsahrs[19],Fd_rsahrs[20],Fd_rsahrs[21],Fd_rsahrs[22]);      //横滚角
			AHRSData_Packet.Pitch=DATA_Trans(Fd_rsahrs[23],Fd_rsahrs[24],Fd_rsahrs[25],Fd_rsahrs[26]);     //俯仰角
			AHRSData_Packet.Heading=DATA_Trans(Fd_rsahrs[27],Fd_rsahrs[28],Fd_rsahrs[29],Fd_rsahrs[30]);	 //偏航角
		}		
		rs_ahrstype=0;
	}	
}
/*******************************
16进制转浮点型数据
*******************************/
u8 TTL_Hex2Dec(void)  
{
	if(rs_ahrstype==1)
	{
		if(Fd_rsahrs[1]==TYPE_AHRS&&Fd_rsahrs[2]==AHRS_LEN)
		{	
			AHRSData_Packet.RollSpeed=DATA_Trans(Fd_rsahrs[7],Fd_rsahrs[8],Fd_rsahrs[9],Fd_rsahrs[10]);       //横滚角速度
			AHRSData_Packet.PitchSpeed=DATA_Trans(Fd_rsahrs[11],Fd_rsahrs[12],Fd_rsahrs[13],Fd_rsahrs[14]);   //俯仰角速度
			AHRSData_Packet.HeadingSpeed=DATA_Trans(Fd_rsahrs[15],Fd_rsahrs[16],Fd_rsahrs[17],Fd_rsahrs[18]); //偏航角速度
				
			AHRSData_Packet.Roll=DATA_Trans(Fd_rsahrs[19],Fd_rsahrs[20],Fd_rsahrs[21],Fd_rsahrs[22]);      //横滚角
			AHRSData_Packet.Pitch=DATA_Trans(Fd_rsahrs[23],Fd_rsahrs[24],Fd_rsahrs[25],Fd_rsahrs[26]);     //俯仰角
			AHRSData_Packet.Heading=DATA_Trans(Fd_rsahrs[27],Fd_rsahrs[28],Fd_rsahrs[29],Fd_rsahrs[30]);	 //偏航角
				
			AHRSData_Packet.Qw=DATA_Trans(Fd_rsahrs[31],Fd_rsahrs[32],Fd_rsahrs[33],Fd_rsahrs[34]);  //四元数
			AHRSData_Packet.Qx=DATA_Trans(Fd_rsahrs[35],Fd_rsahrs[36],Fd_rsahrs[37],Fd_rsahrs[38]);
			AHRSData_Packet.Qy=DATA_Trans(Fd_rsahrs[39],Fd_rsahrs[40],Fd_rsahrs[41],Fd_rsahrs[42]);
			AHRSData_Packet.Qz=DATA_Trans(Fd_rsahrs[43],Fd_rsahrs[44],Fd_rsahrs[45],Fd_rsahrs[46]);
			AHRSData_Packet.Timestamp=timestamp(Fd_rsahrs[47],Fd_rsahrs[48],Fd_rsahrs[49],Fd_rsahrs[50]);   //时间戳
		}
		rs_ahrstype=0;
 }
	if(rs_imutype==1)
	{
		if(Fd_rsimu[1]==TYPE_IMU&&Fd_rsimu[2]==IMU_LEN)
		{
			IMUData_Packet.gyroscope_x=DATA_Trans(Fd_rsimu[7],Fd_rsimu[8],Fd_rsimu[9],Fd_rsimu[10]);  //角速度
			IMUData_Packet.gyroscope_y=DATA_Trans(Fd_rsimu[11],Fd_rsimu[12],Fd_rsimu[13],Fd_rsimu[14]);
			IMUData_Packet.gyroscope_z=DATA_Trans(Fd_rsimu[15],Fd_rsimu[16],Fd_rsimu[17],Fd_rsimu[18]);
				
			IMUData_Packet.accelerometer_x=DATA_Trans(Fd_rsimu[19],Fd_rsimu[20],Fd_rsimu[21],Fd_rsimu[22]);  //线加速度
			IMUData_Packet.accelerometer_y=DATA_Trans(Fd_rsimu[23],Fd_rsimu[24],Fd_rsimu[25],Fd_rsimu[26]);
			IMUData_Packet.accelerometer_z=DATA_Trans(Fd_rsimu[27],Fd_rsimu[28],Fd_rsimu[29],Fd_rsimu[30]);

			IMUData_Packet.magnetometer_x=DATA_Trans(Fd_rsimu[31],Fd_rsimu[32],Fd_rsimu[33],Fd_rsimu[34]);  //磁力计数据
			IMUData_Packet.magnetometer_y=DATA_Trans(Fd_rsimu[35],Fd_rsimu[36],Fd_rsimu[37],Fd_rsimu[38]);
			IMUData_Packet.magnetometer_z=DATA_Trans(Fd_rsimu[39],Fd_rsimu[40],Fd_rsimu[41],Fd_rsimu[42]);
				
			IMUData_Packet.Timestamp=timestamp(Fd_rsimu[55],Fd_rsimu[56],Fd_rsimu[57],Fd_rsimu[58]);   //时间戳
		}
		rs_imutype=0;
 }
return 0;
}
/*************
实现16进制的can数据转换成浮点型数据
****************/
float DATA_Trans(u8 Data_1,u8 Data_2,u8 Data_3,u8 Data_4)
{
  long long transition_32;
	float tmp=0;
	int sign=0;
	int exponent=0;
	float mantissa=0;
  transition_32 = 0;
  transition_32 |=  Data_4<<24;   
  transition_32 |=  Data_3<<16; 
	transition_32 |=  Data_2<<8;
	transition_32 |=  Data_1;
  sign = (transition_32 & 0x80000000) ? -1 : 1;//符号位
	//先右移操作，再按位与计算，出来结果是30到23位对应的e
	exponent = ((transition_32 >> 23) & 0xff) - 127;
	//将22~0转化为10进制，得到对应的x系数 
	mantissa = 1 + ((float)(transition_32 & 0x7fffff) / 0x7fffff);
	tmp=sign * mantissa * pow(2, exponent);
	return tmp;
}
long long timestamp(u8 Data_1,u8 Data_2,u8 Data_3,u8 Data_4)
{
  u32 transition_32;
  transition_32 = 0;
  transition_32 |=  Data_4<<24;   
  transition_32 |=  Data_3<<16; 
	transition_32 |=  Data_2<<8;
	transition_32 |=  Data_1;
	return transition_32;
}
/**************************************************************************
函数功能：将字符形的数字转为float类型
入口参数：特定字母后的字符数字及其长度
返回  值：转换后的数字
**************************************************************************/
float CharToNum(uint8_t *cData,uint8_t len)
{
    uint8_t i=0,dot_count = 0;
    int Num_Return = 0,Num_cnt = 1,Num_Receive;
    for(i=1;i<len+1;i++)
    {
			Num_Receive = *cData-'0';
			switch(dot_count)
			{
					case 0:
						switch(*cData)
						{
								case '.':
									dot_count++;
									break;
								case '-':
									Num_cnt *= -1;
									break;
								default:
									if(Num_Receive>9 || Num_Receive<0) return 0;
									Num_Return = Num_Return*10+Num_Receive;
									break;
						}
						break;
					case 1:
						if(*cData=='.')			//接收到多个‘.’
								return 0;
						else
						{
							if(Num_Receive>9 || Num_Receive<0) return 0;
									Num_Return = Num_Return*10+Num_Receive;
							Num_cnt*=10;
						}							
						break;
					default:
						return 0;
			}
			cData++;
    }
    return Num_Return/1.0/Num_cnt;
}
